#include "types.h"
#include "state.h"
#include "sensors_HMC5883.h"
#include "driverLog.h"
#include "driverI2C_ISR.h"
#include "sensors.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "config.h"
#include "main_peripherals.h"



#define MAGN_AXIS_X 0
#define MAGN_AXIS_Y 4
#define MAGN_AXIS_Z 2
#define MAGN_AXIS_X_SIGN +
#define MAGN_AXIS_Y_SIGN +
#define MAGN_AXIS_Z_SIGN +

#define HMC5883_ADDRESS		0x3C


// ADC = 12 bits

// 6 bytes receive buffer for i2c (x, y, z)
BYTE				magnBuffer[6];
BYTE                magnReadRegister;
// i2c action triggered on interrupt (INT1)
struct i2cAction_t  magn;


/**
 * Callback from I2C ISR > data received
 *
 * !! Should not do lots of stuff !!
 */
void magnCallback(portBASE_TYPE * higherPrior)
{
    if (magn.state != I2C_STATE_DONE)
    {
        STATE.hmc5883.failures++;
        //STATE.hmc5883.connected = 0;
        return;
    }

    STATE.hmc5883.totalSamples++;

    BYTE temp = magnBuffer[0];
    magnBuffer[0] = magnBuffer[1];
    magnBuffer[1] = temp;
    temp = magnBuffer[2];
    magnBuffer[2] = magnBuffer[3];
    magnBuffer[3] = temp;
    temp = magnBuffer[4];
    magnBuffer[4] = magnBuffer[5];
    magnBuffer[5] = temp;

    // UNSAFE
    STATE.hmc5883.buffer[STATE.hmc5883.bufferPosition] = (SHORT3){
            MAGN_AXIS_X_SIGN(short)(*(short*)&(magnBuffer[MAGN_AXIS_X])),
            MAGN_AXIS_Y_SIGN(short)(*(short*)&(magnBuffer[MAGN_AXIS_Y])),
            MAGN_AXIS_Z_SIGN(short)(*(short*)&(magnBuffer[MAGN_AXIS_Z]))};

    logStore(xTaskGetTickCountFromISR(), LOG_ID_MAGN_RAW, STATE.hmc5883.buffer[STATE.hmc5883.bufferPosition]);

    STATE.hmc5883.bufferPosition++;
    if (STATE.hmc5883.bufferPosition == MAGN_BUFFERSIZE)
    {
            STATE.hmc5883.bufferPosition = 0;
            STATE.hmc5883.bufferFilled = 1;
    }
}

/**
 * Checks if HMC5883 who-am-i register is correct.
 *
 * @return BOOL true if succeeded
 */
BOOL magnCheck()
{
    struct i2cAction_t * result;
    result = sensorRead(HMC5883_ADDRESS,0x0A,0x00);
    if (result->state != I2C_STATE_DONE || result->readPacket[0] != 'H') return FALSE;
    result = sensorRead(HMC5883_ADDRESS,0x0B,0x00);
    if (result->state != I2C_STATE_DONE || result->readPacket[0] != '4') return FALSE;
    result = sensorRead(HMC5883_ADDRESS,0x0C,0x00);
    if (result->state != I2C_STATE_DONE || result->readPacket[0] != '3') return FALSE;

    return TRUE;
}


/**
 * Configures the hmc5883 mode
 *
 * @return BOOL true if succeeded
 */
BOOL magnSetMode(BYTE mode)
{
    struct i2cAction_t * result;

    result = sensorWrite(HMC5883_ADDRESS, 0x02, mode);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}

BOOL magnConfigure()
{
    struct i2cAction_t * result;
    BYTE c1 = ((CONFIG.hmc5883.hardwareFreq & 0x07) << 2) |
              ((CONFIG.hmc5883.hardwareAvg & 0x03) << 5);
    BYTE c2 = ((CONFIG.hmc5883.hardwareGain & 0x07) << 5);

    result = sensorWrite(HMC5883_ADDRESS, 0x00, c1);
    if (result->state != I2C_STATE_DONE) return FALSE;
    result = sensorWrite(HMC5883_ADDRESS, 0x01, c2);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}

void magnChangedFreq(const void * ref, const void * const value)
{
    CONFIG.hmc5883.hardwareFreq = *(BYTE*)value;
    magnChanged();
}

void magnChangedGain(const void * ref, const void * const value)
{
    CONFIG.hmc5883.hardwareGain = *(BYTE*)value;
    magnChanged();
}

void magnChangedAvg(const void * ref, const void * const value)
{
    CONFIG.hmc5883.hardwareAvg = *(BYTE*)value;
    magnChanged();
}

void magnChanged()
{
    STATE.hmc5883.connected = 0;
    STATE.hmc5883.calibrated = 0;
    STATE.hmc5883.bufferFilled = 0;
    STATE.hmc5883.bufferPosition = 0;

    while (!magnInit())
    {
        vTaskDelay(10);
    }

    STATE.hmc5883.connected = 1;

    i2cQueueAction(&magn);
}

/**
 * Initializes all the necessary hmc registers
 *
 * @return BOOL true if succeeded
 */
BOOL magnInit()
{
    IEC0CLR = _IEC0_INT1IE_MASK;
    IFS0CLR = _IFS0_INT1IF_MASK;
 	//ConfigINT1(EXT_INT_DISABLE | FALLING_EDGE_INT | EXT_INT_PRI_5);

    // Read WHO AM I register
    if (!magnCheck()) return FALSE;

    // Set device in idle mode
    if (!magnSetMode(0x03)) return FALSE;
    vTaskDelay(50);

    // Configure A & B reg (freq, avg, gain)
    if (!magnConfigure()) return FALSE;

    // Set device in highspeed & continuous mode
    if (!magnSetMode(0x80)) return FALSE;


    INTCONCLR = _INTCON_INT1EP_MASK;
    IEC0SET = _IEC0_INT1IE_MASK;
    IPC1bits.INT1IP = 5;
 	//ConfigINT1(EXT_INT_ENABLE | FALLING_EDGE_INT | EXT_INT_PRI_5);

    return TRUE;
}

/**
 * Called by driverSensors -> sensorInit
 *
 * !! Will block if mpu is not detected !!
 */
void magnSetup()
{
    while (!magnInit())
    {
        // 1 long, 1 short
        BUZZ(BUZZ_DELAY_ERROR, 1);
        BUZZ(BUZZ_DELAY_NOTIFY, 2);

        vTaskDelay(configTICK_RATE_HZ);
    }

    magnReadRegister = 0x03;

    magn.address = HMC5883_ADDRESS;
    magn.writeLength = 1;
    magn.writePacket = &magnReadRegister;
    
    magn.readLength = 6;
    magn.readPacket = magnBuffer;
    magn.callbackFunction = &magnCallback;

    magn.state = I2C_STATE_DONE;
    STATE.hmc5883.connected = 1;
}

void __attribute__((vector(_EXTERNAL_1_IRQ), interrupt(ipl5))) ISRWRAPPER_INT1();

/**
 * INT1 > Data ready interrupt
 */
void ISR_INT1()
{
    IFS0CLR = _IFS0_INT1IF_MASK;
    //INTClearFlag(INT_INT1);
    
    if (STATE.hmc5883.connected)
    {
        if (magn.state >= I2C_STATE_DONE)
        {
            i2cQueueActionFromISR(&magn);
        }
    }

    portEND_SWITCHING_ISR(0);
}
